﻿
// 2021/12/2: 增加俯仰角速度和横滚角速度。基于gnssimu-sample-v6.h (版本2020/11/11)
// 2021/12/16: 增加水平误差和垂直误差
// 2022/3/16: 增加行驶曲率
// 2022/11/2: 支持Spadas8.4
// 2022/11/14: 实现插值
// 2023/10/11: 补充部分英文注释
// 2023/11/14: 增加TAI时间类型。增加东北天向速度获取函数enuSpeed
// 2024/7/3: 基于新样本框架。枚举改为使用enum class
// 2025/8/4: 不使用cat宏

#ifndef GNSSIMU_SAMPLE_V7_H
#define GNSSIMU_SAMPLE_V7_H

#include "spadas.h"

namespace gnssimu_sample_v7
{
	using namespace spadas;

	enum class GnssImuLocationMode
	{
		NoLocation = 0, // 无位置信息
		Normal = 1, // 默认模式
		RTKFixed = 2, // RTK固定解
		RTKFloat = 3, // RTK浮动解
		RTD = 4, // RTD
		IMUOnly = 5, // 仅惯导
		Modified = 6, // 后期修正
	};

	enum class GnssTimeType
	{
		UTC = 0, // UTC时间
		GPS = 1, // GPS时间
		TAI = 2, // TAI时间
	};

	struct GnssImuSampleData
	{
		GnssImuLocationMode locationMode; // 位置模式
		OptionalInt satelliteCount; // 卫星数量

		OptionalDouble longitude; // [°E] Longitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的经度
		OptionalDouble latitude; // [°N] Latitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的纬度
		OptionalDouble altitude; // [m] Altitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的海拔

		OptionalDouble antennaLongitude; // [°E] Longitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的经度
		OptionalDouble antennaLatitude; // [°N] Latitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的纬度
		OptionalDouble antennaAltitude; // [m] 天线位置海拔Altitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的海拔

		OptionalDouble horizontalError; // [m] 水平位置误差
		OptionalDouble verticalError; // [m] 垂直位置误差

		OptionalDouble speed; // [kph] 车速

		OptionalDouble orientation; // [deg -180~180] 0 as north, CCW is positive / 朝向 CCW为正 北为0
		OptionalDouble pitch; // [deg] Heading down is positive / 俯仰角 车头朝下为正
		OptionalDouble roll; // [deg] Right side heading down is positive / 横滚角 右侧朝下为正

		OptionalDouble yawRate; // [deg/s] Turning left is positive / 横摆角速度 左转为正
		OptionalDouble pitchRate; // [deg/s] Heading down is positive / 俯仰角速度 车头朝下为正
		OptionalDouble rollRate; // [deg/s] Right side heading down is positive / 横滚角速度 右侧朝下为正

		OptionalDouble ax; // [m/s2] Front side is positive / 纵向加速度 朝前为正
		OptionalDouble ay; // [m/s2] Left side is positive / 横向加速度 朝左为正
		OptionalDouble az; // [m/s2] Top side is positive / 天向加速度 朝上为正

		OptionalDouble curvature; // [1/m] Turning left is positive / 行驶曲率 左转为正

		OptionalDouble slipAngle; // [deg] Turning left is positive / 侧偏角 左转时为正
		OptionalDouble jerkX; // [m/s3] Front side is positive / 纵向急动度  朝前为正
		OptionalDouble jerkY; // [m/s3] Left side is positive / 横向急动度 朝左为正

		OptionalDouble arrivalTime; // [s] Arrival time offset (Relative to the session) / 数据到达时间戳（相对Session开始时间）
		GnssTimeType gnssTimeType; // 卫星时间类型
		Optional<TimeWithMS> gnssTime; // 卫星时间

		GnssImuSampleData()
		{
			locationMode = GnssImuLocationMode::NoLocation;
			gnssTimeType = GnssTimeType::UTC;
		}

		Optional<TimeWithMS> gnssTimeUTC()
		{
			if (gnssTime.isValid())
			{
				if (gnssTimeType == GnssTimeType::UTC) return gnssTime;
				else if (gnssTimeType == GnssTimeType::GPS) return gnssTime.value() + (-18000);
				else if (gnssTimeType == GnssTimeType::TAI) return gnssTime.value() + (-37000);
			}
			return Optional<TimeWithMS>();
		}

		Optional<Vector3D> enuSpeed() // [kph]
		{
			if (speed.valid && orientation.valid && pitch.valid)
			{
				Vector3D v;
				v.z = -speed.value * math::sin(pitch.value);
				Double hSpeed = speed.value * math::cos(pitch.value);
				v.y = hSpeed * math::cos(orientation.value);
				v.x = -hSpeed * math::sin(orientation.value);
				return v;
			}
			else return Optional<Vector3D>();
		}
	};

	class GnssImuSample : public Object<class GnssImuSampleVars>
	{
	public:
		SPADAS_TYPE("gnssimu_sample_v7.GnssImuSample")

		GnssImuSample();
		FullTimestamp& timestamp();
		GnssImuSampleData *operator ->();

		static String getProtocolName(UInt channel);
		SessionGeneralSample toGeneralSample();

		static Array<String> getProtocolNames(UInt channel);
		Bool fromGeneralSample(String protocol, SessionGeneralSample generalSample);
	};
}

#endif
